#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "cv_bridge/cv_bridge.h"
#include <opencv2/opencv.hpp>

class MyNode : public rclcpp::Node
{
public:
    MyNode() : Node("mynode_node_cpp")
    {
        sub_ = this->create_subscription<sensor_msgs::msg::Image>(
            "/image_out", 5, std::bind(&MyNode::img_back, this, std::placeholders::_1));
    }

private:
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;

    void img_back(const sensor_msgs::msg::Image::SharedPtr img_msg)
    {
        try
        {
            // 将ROS图像消息转换为OpenCV图像
            auto cv_img = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::BGR8);
            // 创建一个窗口并显示图像
            cv::imshow("Image window", cv_img->image);
            // 等待1毫秒，以便图像有时间显示
            cv::waitKey(1);
        }
        catch (cv_bridge::Exception& e)
        {
            RCLCPP_ERROR(this->get_logger(), "Could not convert from '%s' to 'bgr8'.", img_msg->encoding.c_str());
        }
    }
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MyNode>());
    rclcpp::shutdown();
    return 0;
}